#include <sys/types.h>
#include <sstream>
#include <unistd.h>



#include <ros/ros.h>
#include <std_msgs/String.h>
#include <serial/serial.h>
#include <XmlRpcException.h>

#include <sensor_msgs/Imu.h>
#include <sensor_msgs/NavSatFix.h>
#include <sensor_msgs/NavSatStatus.h>


#include "INS570D/INS570D_Driver.h"
#include "M2/INSM2_Driver.h"
#include "insdriverpublisher.h"

bool openSerialPort(serial::Serial &serial_,
                    const std::string serial_port_,
                    const uint32_t baud_rate_,
                    serial::Timeout to = serial::Timeout::simpleTimeout(10)){
    serial_.setPort(serial_port_);
    //设置串口通信的波特率
    serial_.setBaudrate(230400);
    //串口设置timeout
    serial_.setTimeout(to);

    try
    {
        //打开串口
        serial_.open();
    }
    catch(serial::IOException& e)
    {
        ROS_ERROR_STREAM("Unable to open port.");
        return -1;
    }

    //判断串口是否打开成功
    if(serial_.isOpen())
    {
        ROS_INFO_STREAM(serial_port_<<" is opened.");
    }
    else
    {
        return -1;
    }
}

int main(int argc, char **argv)
{

    ros::init(argc, argv, "gnssimu");
    ros::NodeHandle nh;                             //用来发布topic的
    ros::NodeHandle nh_("/driver_config");          //用来获取参数的

    //创建一个serial类
    serial::Serial sp;
    //创建timeout
    serial::Timeout to = serial::Timeout::simpleTimeout(10);
    //设置要打开的串口名称
    std::string serial_port;
    int baud_rate;

    //============================获取Launch文件参数=========================
    std::string GPS_frame_id,IMU_frame_id,map_frame_id;
    bool use_GNSS_time=false;
    double gravity_constant;
    std::string insDriver_type;
    nh_.param("port",serial_port,std::string("/dev/ttyACM0"));
    nh_.param("baud",baud_rate,115200);
    nh_.param("GPS_frame_id",GPS_frame_id,std::string("/gps"));
    nh_.param("IMU_frame_id",IMU_frame_id,std::string("/imu_link"));
    nh_.param("map_frame_id",map_frame_id,std::string("/map"));
    nh_.param("gravity_constant",gravity_constant,9.81);
    nh_.param("insDriver_type",insDriver_type,std::string("M2"));

    ROS_INFO("Serial Port: %s",serial_port.c_str());
    ROS_INFO("baud_rate: %d",baud_rate);
    ROS_INFO("GPS_frame_id: %s",GPS_frame_id.c_str());
    ROS_INFO("IMU_frame_id: %s",IMU_frame_id.c_str());
    ROS_INFO("map_frame_id: %s",map_frame_id.c_str());
    ROS_INFO("gravity_constant: %f",gravity_constant);
    ROS_INFO("insDriver_type: %s",insDriver_type.c_str());

    //============================打开串口==================================
    openSerialPort(sp,serial_port,baud_rate,to);


    //============================解析和发布================================

    std::shared_ptr<insDriverPublisher> publisher_=std::make_shared<insDriverPublisher>(nh);
    std::shared_ptr<BufferParaseInterface> m2_driver=std::make_shared<INSM2_Driver>();
    std::shared_ptr<BufferParaseInterface> driver_= std::make_shared<INS570D_Driver>(58);
    InsDriver_type driver_type;
    if(insDriver_type=="M2"){
        driver_type=InsDriver_type::M2;
    }else if(insDriver_type=="ins570D"){
        driver_type=InsDriver_type::ins570D;
        std::cout<<"use INS570D"<<std::endl;
    }
    ros::Rate loop_rate(1000);      // 循环频率，数值越大，频率越快
    while(ros::ok())
    {
        if(driver_type==InsDriver_type::ins570D){

            /// TODO : buffer缓冲队列
            std::vector<uint8_t> buf;
            //std::cout<<"Start Reading"<<std::endl;
            sp.read(buf,58);
            if(buf.size()==58){
                bool checksum=driver_->setBuffer(buf);
                if(checksum){
                    driver_->parse();
//                    std::cout<<"test1"<<std::endl;
                }
                publisher_->publish(driver_->getInsData());
            }
        }else if(driver_type==InsDriver_type::M2){
            std::string sentense;
            try
            {
                // use try
                sp.readline(sentense);
            }catch(serial::IOException& e)
            {
                ROS_ERROR_STREAM("ROS serial::IOException ERROR");
                sp.close();
                sleep(3);
                openSerialPort(sp,serial_port,baud_rate,to);
                continue;
            }catch(serial::SerialException& e)
            {
                ROS_ERROR_STREAM("ROS SerialException ERROR");
                sp.close();
                sleep(3);
                openSerialPort(sp,serial_port,baud_rate,to);
                continue;
            }


            if(sentense.size()>0){
                m2_driver->setBuffer(sentense);
                m2_driver->parse();
                publisher_->publish(m2_driver->getInsData(),m2_driver->getIMUData(),m2_driver->getRTKData());
            }
        }
        loop_rate.sleep();
    }
    //关闭串口
    sp.close();
    ros::spin();
    return 0;
}
